#include "Common/Algebra/Matr4.h"

namespace cmn {

	Matr4::Matr4() {
		for( int i = 0; i < 16; ++i )
			m_array[i] = 0.0f;

		m_array[0] = 1.0f;
		m_array[5] = 1.0f;
		m_array[10] = 1.0f;
		m_array[15] = 1.0f;
	}

	Matr4::Matr4( const Matr4& matr ) {
		for( int i = 0; i < 16; ++i )
			m_array[i] = matr[i];
	}

	Matr4::Matr4( float* array ) {
		for( int i = 0; i < 16; ++i )
			m_array[i] = array[i];
	}

	Matr4::Matr4(
		float a11, float a12, float a13, float a14,
		float a21, float a22, float a23, float a24,
		float a31, float a32, float a33, float a34,
		float a41, float a42, float a43, float a44 )
	{
		m_array[0] = a11;
		m_array[4] = a12;
		m_array[8] = a13;
		m_array[12] = a14;
		m_array[1] = a21;
		m_array[5] = a22;
		m_array[9] = a23;
		m_array[13] = a24;
		m_array[2] = a31;
		m_array[6] = a32;
		m_array[10] = a33;
		m_array[14] = a34;
		m_array[3] = a41;
		m_array[7] = a42;
		m_array[11] = a43;
		m_array[15] = a44;
	}

	Matr4::Matr4( Vec4 col0, Vec4 col1, Vec4 col2, Vec4 col3 ) {
		m_array[0] = col0.X;
		m_array[4] = m_array[0];
		m_array[8] = m_array[4];
		m_array[12] = m_array[8];
		m_array[1] = col0.Y;
		m_array[5] = m_array[1];
		m_array[9] = m_array[5];
		m_array[13] = m_array[9];
		m_array[2] = col0.Z;
		m_array[6] = m_array[2];
		m_array[10] = m_array[6];
		m_array[14] = m_array[10];
		m_array[3] = col0.W;
		m_array[7] = m_array[3];
		m_array[11] = m_array[7];
		m_array[15] = m_array[11];
	}

	Matr4 Matr4::GetInverted() const {
		float a[16];

		for( int i = 0; i < 16; ++i )
			a[i] = m_array[i];

		float coef = 1.0f / (
						 a[0] * a[10] * a[13] * a[7]
						 - a[0] * a[10] * a[15] * a[5]
						 + a[11] * (
							 -a[0] * a[13] * a[6]
							 + a[0] * a[14] * a[5]
							 - a[12] * a[2] * a[5]
							 + a[13] * a[2] * a[4] )
						 - a[0] * a[14] * a[7] * a[9]
						 + a[0] * a[15] * a[6] * a[9]
						 + a[1] * (
							 -a[10] * a[12] * a[7]
							 + a[10] * a[15] * a[4]
							 + a[11] * a[12] * a[6]
							 - a[11] * a[14] * a[4]
							 + a[14] * a[7] * a[8]
							 - a[15] * a[6] * a[8] )
						 + a[10] * a[12] * a[3] * a[5]
						 - a[10] * a[13] * a[3] * a[4]
						 + a[12] * a[2] * a[7] * a[9]
						 - a[12] * a[3] * a[6] * a[9]
						 - a[13] * a[2] * a[7] * a[8]
						 + a[13] * a[3] * a[6] * a[8]
						 + a[14] * a[3] * a[4] * a[9]
						 - a[14] * a[3] * a[5] * a[8]
						 - a[15] * a[2] * a[4] * a[9]
						 + a[15] * a[2] * a[5] * a[8] );

		float array[16];

		array[0] =  a[11] * a[14] * a[5] - a[10] * a[15] * a[5] - a[11] * a[13] * a[6] + a[10] * a[13] * a[7] + a[15] * a[6] * a[9] - a[14] * a[7] * a[9];
		array[1] = -a[1] * a[11] * a[14] + a[3] * a[9] * a[14] + a[1] * a[10] * a[15] + a[11] * a[13] * a[2] - a[10] * a[13] * a[3] - a[15] * a[2] * a[9];
		array[2] = a[15] * a[2] * a[5] - a[14] * a[3] * a[5] - a[1] * a[15] * a[6] + a[13] * a[3] * a[6] + a[1] * a[14] * a[7] - a[13] * a[2] * a[7];
		array[3] = -a[11] * a[2] * a[5] + a[10] * a[3] * a[5] + a[1] * a[11] * a[6] - a[1] * a[10] * a[7] - a[3] * a[6] * a[9] + a[2] * a[7] * a[9];
		array[4] =  -a[11] * a[14] * a[4] + a[10] * a[15] * a[4] + a[11] * a[12] * a[6] - a[10] * a[12] * a[7] - a[15] * a[6] * a[8] + a[14] * a[7] * a[8];
		array[5] =  a[0] * a[11] * a[14] - a[3] * a[8] * a[14] - a[0] * a[10] * a[15] - a[11] * a[12] * a[2] + a[10] * a[12] * a[3] + a[15] * a[2] * a[8];
		array[6] =  -a[15] * a[2] * a[4] + a[14] * a[3] * a[4] + a[0] * a[15] * a[6] - a[12] * a[3] * a[6] - a[0] * a[14] * a[7] + a[12] * a[2] * a[7];
		array[7] =  a[11] * a[2] * a[4] - a[10] * a[3] * a[4] - a[0] * a[11] * a[6] + a[0] * a[10] * a[7] + a[3] * a[6] * a[8] - a[2] * a[7] * a[8];
		array[8] =   a[11] * a[13] * a[4] - a[15] * a[9] * a[4] - a[11] * a[12] * a[5] + a[15] * a[5] * a[8] - a[13] * a[7] * a[8] + a[12] * a[7] * a[9];
		array[9] =  a[1] * a[11] * a[12] - a[3] * a[9] * a[12] - a[0] * a[11] * a[13] - a[1] * a[15] * a[8] + a[13] * a[3] * a[8] + a[0] * a[15] * a[9];
		array[10] =  a[1] * a[15] * a[4] - a[13] * a[3] * a[4] - a[0] * a[15] * a[5] + a[12] * a[3] * a[5] - a[1] * a[12] * a[7] + a[0] * a[13] * a[7];
		array[11] =  -a[1] * a[11] * a[4] + a[3] * a[9] * a[4] + a[0] * a[11] * a[5] - a[3] * a[5] * a[8] + a[1] * a[7] * a[8] - a[0] * a[7] * a[9];
		array[12] =  -a[10] * a[13] * a[4] + a[14] * a[9] * a[4] + a[10] * a[12] * a[5] - a[14] * a[5] * a[8] + a[13] * a[6] * a[8] - a[12] * a[6] * a[9];
		array[13] =  -a[1] * a[10] * a[12] + a[2] * a[9] * a[12] + a[0] * a[10] * a[13] + a[1] * a[14] * a[8] - a[13] * a[2] * a[8] - a[0] * a[14] * a[9];
		array[14] =  -a[1] * a[14] * a[4] + a[13] * a[2] * a[4] + a[0] * a[14] * a[5] - a[12] * a[2] * a[5] + a[1] * a[12] * a[6] - a[0] * a[13] * a[6];
		array[15] =  a[1] * a[10] * a[4] - a[2] * a[9] * a[4] - a[0] * a[10] * a[5] + a[2] * a[5] * a[8] - a[1] * a[6] * a[8] + a[0] * a[6] * a[9];

		for( int i = 0; i < 16; ++i )
			array[i] *= coef;

		return Matr4( array );
	}

	Matr4 Matr4::operator += ( const Matr4& m ) {
		for( int i = 0; i < 16; ++i )
			m_array[i] += m[i];
		return *this;
	}

	Matr4 Matr4::operator -= ( const Matr4& m ) {
		for( int i = 0; i < 16; ++i )
			m_array[i] -= m[i];
		return *this;
	}

	Matr4 Matr4::operator *= ( const Matr4& m ) {
		float array[16];

		for( int i = 0; i < 16; ++i )
			array[i] = m_array[i];

		m_array[0] = array[0] * m[0] + array[4] * m[1] + array[8] * m[2] + array[12] * m[3];
		m_array[1] = array[1] * m[0] + array[5] * m[1] + array[9] * m[2] + array[13] * m[3];
		m_array[2] = array[2] * m[0] + array[6] * m[1] + array[10] * m[2] + array[14] * m[3];
		m_array[3] = array[3] * m[0] + array[7] * m[1] + array[11] * m[2] + array[15] * m[3];

		m_array[4] = array[0] * m[4] + array[4] * m[5] + array[8] * m[6] + array[12] * m[7];
		m_array[5] = array[1] * m[4] + array[5] * m[5] + array[9] * m[6] + array[13] * m[7];
		m_array[6] = array[2] * m[4] + array[6] * m[5] + array[10] * m[6] + array[14] * m[7];
		m_array[7] = array[3] * m[4] + array[7] * m[5] + array[11] * m[6] + array[15] * m[7];

		m_array[8] = array[0] * m[8] + array[4] * m[9] + array[8] * m[10] + array[12] * m[11];
		m_array[9] = array[1] * m[8] + array[5] * m[9] + array[9] * m[10] + array[13] * m[11];
		m_array[10] = array[2] * m[8] + array[6] * m[9] + array[10] * m[10] + array[14] * m[11];
		m_array[11] = array[3] * m[8] + array[7] * m[9] + array[11] * m[10] + array[15] * m[11];

		m_array[12] = array[0] * m[12] + array[4] * m[13] + array[8] * m[14] + array[12] * m[15];
		m_array[13] = array[1] * m[12] + array[5] * m[13] + array[9] * m[14] + array[13] * m[15];
		m_array[14] = array[2] * m[12] + array[6] * m[13] + array[10] * m[14] + array[14] * m[15];
		m_array[15] = array[3] * m[12] + array[7] * m[13] + array[11] * m[14] + array[15] * m[15];
		return *this;
	}

	Matr4 Matr4::operator *= ( float a ) {
		for( int i = 0; i < 16; ++i )
			m_array[i] *= a;
		return *this;
	}

	Matr4 Matr4::operator /= ( float a ) {
		for( int i = 0; i < 16; ++i )
			m_array[i] /= a;
		return *this;
	}

	float Matr4::GetDeterminant() const {
		return
			m_array[0] * m_array[5] * m_array[10] +
			m_array[2] * m_array[4] * m_array[9] +
			m_array[1] * m_array[6] * m_array[8] -
			m_array[2] * m_array[5] * m_array[8] -
			m_array[1] * m_array[4] * m_array[10] -
			m_array[0] * m_array[6] * m_array[9];
	}

	Matr4 Matr4::GetTransposed() const {
		return Matr4(
				   m_array[0], m_array[4], m_array[8], m_array[12],
				   m_array[1], m_array[5], m_array[9], m_array[13],
				   m_array[2], m_array[6], m_array[10], m_array[14],
				   m_array[3], m_array[7], m_array[11], m_array[15] );
	}
}
